#pragma

#include <opencv2/aruco.hpp>
#include <opencv2/opencv.hpp>

#include "camera_conf.h"

#include "parameter.h"

class Result_Pose_estimation
{   
    public:
    cv::Vec3d rvec, tvec;
    std::vector< cv::Vec3d > rvecs, tvecs; 
};

namespace aruco_node
{

class ArucoTest
{
    public:
        ParameterReader pd;

        cv::Mat markerImage;
        int arucoFormatId_;
        double markerLength; 
        cv::Ptr<cv::aruco::Dictionary> dict;

        std::vector<int> markerIds;
        std::vector< std::vector<cv::Point2f> > markerCorners, rejectedCandidates;
        cv::Ptr<cv::aruco::DetectorParameters> parameters;
        
        cv::Ptr<cv::aruco::GridBoard> board;
        int markersX, markersY;
        double markerSaparation;

    public:
        ArucoTest(int arucoFormatId);

        ~ArucoTest(){};

        void create_aruco(int id, int longth);
        void create_aruco_id(int id, int longth);
        void detete_aruco(cv::Mat& inputImage);
        void save_arurco(int id, int length);
        void pose_estimation(cv::Mat& inputImage,CameraConfig& camera_conf, Result_Pose_estimation& result);
        void pose_grid_board(cv::Mat& inputImage, CameraConfig& camera_conf, Result_Pose_estimation& result);

        void vector2Mat(cv::Vec3d rvec, cv::Vec3d tvec);
};

}